#include "oberon7_robot_driver/oberon7_hardware.hpp"

namespace oberon7_robot_driver
{

  static const std::string HW_NAME = "OberonPosHardwareInterface";


  Oberon7Hardware::Oberon7Hardware():
    _robot(),
    hw_commands_(6), 
    hw_states_position_(6), hw_states_velocity_(6), hw_states_effort_(6)
  {
    // rclcpp::init(0, 0);
    // _robot = Oberon7Driver();
  }

  void Oberon7Hardware::writeCommandsToHardware()
  {
    // call MBZIRC robot API to set joint values
    _robot.SendJointCommand(hw_commands_);
  }


  void Oberon7Hardware::updateJointsFromHardware()
  {
    // call MBZIRC robot API to get joint values
    auto position = _robot.GetJointPosition();
    hw_states_position_.swap(position);
    auto velocity = _robot.GetJointVelocity();
    hw_states_velocity_.swap(velocity);
    auto effort = _robot.GetJointEffort();
    hw_states_effort_.swap(effort);
  }


  CallbackReturn
  Oberon7Hardware::on_init(const hardware_interface::HardwareInfo &system_info)
  {
    if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS)
    {
      return CallbackReturn::ERROR;
    }

    info_ = system_info;

    for (const hardware_interface::ComponentInfo &joint : info_.joints)
    {
      if (joint.command_interfaces.size() != 1)
      {
        RCLCPP_FATAL(rclcpp::get_logger(HW_NAME),
                     "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
                     joint.command_interfaces.size());
        return CallbackReturn::ERROR;
      }

      if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
      {
        RCLCPP_FATAL(rclcpp::get_logger(HW_NAME),
                     "Joint '%s' have %s command interfaces found as first command interface. '%s' expected.",
                     joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
        return CallbackReturn::ERROR;
      }

      if (joint.state_interfaces.size() != 3)
      {
        RCLCPP_FATAL(rclcpp::get_logger(HW_NAME), "Joint '%s' has %zu state interface. 3 expected.",
                     joint.name.c_str(), joint.state_interfaces.size());
        return CallbackReturn::ERROR;
      }

      if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
      {
        RCLCPP_FATAL(rclcpp::get_logger(HW_NAME),
                     "Joint '%s' have %s state interface as first state interface. '%s' expected.", joint.name.c_str(),
                     joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
        return CallbackReturn::ERROR;
      }

      if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY)
      {
        RCLCPP_FATAL(rclcpp::get_logger(HW_NAME),
                     "Joint '%s' have %s state interface as second state interface. '%s' expected.", joint.name.c_str(),
                     joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
        return CallbackReturn::ERROR;
      }

      if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT)
      {
        RCLCPP_FATAL(rclcpp::get_logger(HW_NAME),
                     "Joint '%s' have %s state interface as third state interface. '%s' expected.", joint.name.c_str(),
                     joint.state_interfaces[2].name.c_str(), hardware_interface::HW_IF_EFFORT);
        return CallbackReturn::ERROR;
      }
    }

    return CallbackReturn::SUCCESS;
  }

  CallbackReturn
  Oberon7Hardware::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
  {
    RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Configuring ...please wait...");    

    RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Successfully configured!");

    return CallbackReturn::SUCCESS;
  }

  std::vector<hardware_interface::StateInterface>
  Oberon7Hardware::export_state_interfaces()
  {
    std::vector<hardware_interface::StateInterface> state_interfaces;
    for (auto i = 0u; i < info_.joints.size(); i++)
    {
      state_interfaces.emplace_back(hardware_interface::StateInterface(
          info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_position_[i]));
      state_interfaces.emplace_back(hardware_interface::StateInterface(
          info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i]));
      state_interfaces.emplace_back(hardware_interface::StateInterface(
          info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_effort_[i]));
    }

    return state_interfaces;
  }

  std::vector<hardware_interface::CommandInterface>
  Oberon7Hardware::export_command_interfaces()
  {
    std::vector<hardware_interface::CommandInterface> command_interfaces;

    for (auto i = 0u; i < info_.joints.size(); i++)
    {
      command_interfaces.emplace_back(hardware_interface::CommandInterface(
          info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
    }

    return command_interfaces;
  }

  CallbackReturn
  Oberon7Hardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
  {
    // set some default values
    for (auto i = 0u; i < hw_states_position_.size(); i++)
    {
      if (std::isnan(hw_states_position_[i]))
      {
        hw_states_position_[i] = 0;
        hw_states_velocity_[i] = 0;
        hw_states_effort_[i] = 0;
        hw_commands_[i] = 0;
      }
    }

    RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "System Successfully started!");

    return CallbackReturn::SUCCESS;
  }

  CallbackReturn
  Oberon7Hardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
  {
    RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "System successfully stopped!");

    return CallbackReturn::SUCCESS;
  }


  hardware_interface::return_type 
  Oberon7Hardware::read()
  {
    RCLCPP_DEBUG(rclcpp::get_logger(HW_NAME), "Reading from hardware");

    updateJointsFromHardware();

    RCLCPP_DEBUG(rclcpp::get_logger(HW_NAME), "Joints successfully read!");

    return hardware_interface::return_type::OK;
  }


  hardware_interface::return_type 
  Oberon7Hardware::write()
  {
    RCLCPP_DEBUG(rclcpp::get_logger(HW_NAME), "Writing to hardware");

    writeCommandsToHardware();

    RCLCPP_DEBUG(rclcpp::get_logger(HW_NAME), "Joints successfully written!");

    return hardware_interface::return_type::OK;
  }

} // namespace oberon7_robot_driver


#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
  oberon7_robot_driver::Oberon7Hardware, hardware_interface::SystemInterface)